package kinectData.models;

import java.util.ArrayList;
import java.util.Collection;

import javax.vecmath.Point3d;

import kinectData.models.Skeleton.Limb;

/**
 * Methods to compare a set of devices with a set of positions and output their similarity.
 * 
 * @author Lauren Norrie (norrielm)
 */
public class Matcher {

	// Match should be within some max value.
	private static final double MAX_MAGNITUDE_DIFF = 0.2;
	// Comparisons should be within the same sample rate.
	private static final double MAX_TIME_DIFF = 50;
	// Devices must be moving to be compared.
	private static final double MIN_POWER = 3;
	
	public static synchronized void compare(Collection<Device> devices, 
			Collection<Skeleton> skeletons, Receiver<Match> callback) {
		ArrayList<Match> allComparisons = new ArrayList<Match>();
		
		for (Device device : devices) {
			// TODO(norrielm): Perform this over the position history rather than individual points.
			AccelerometerData deviceData = new AccelerometerData(device.getPosition());
			for (Skeleton skeleton : skeletons) {
				AccelerometerData positionData = new AccelerometerData(skeleton.getAcceleration(Limb.HANDRIGHT));
				if (inTimeRange(deviceData, positionData) && inSignalRange(deviceData, positionData)) {
					double closeness = Math.abs(magnitude(deviceData.getDataPoint()) - 
						magnitude(positionData.getDataPoint()));
					Match m = new Match(skeleton, device, closeness);
					//TODO(loechti): Add proper minimum for matching... 
					allComparisons.add(m);
				}
			}
		}
		
		ArrayList<Match> closestMatches = new ArrayList<Match>();
		
		while (!allComparisons.isEmpty()) {
			double minVal = 666.0;
			// Find the current closest match in the set.
			Match currentClosest = null;
			for (Match m : allComparisons) {
				if (minVal > Math.abs(m.getMatchingValue())) {
					minVal = Math.abs(m.getMatchingValue());
					currentClosest = m;
				}
			}
			// Remove other matches from the set with this skeleton or device.
			if (currentClosest != null) {
				closestMatches.add(currentClosest);
				ArrayList<Match> removeMatches = new ArrayList<Match>();
				for (Match m : allComparisons){
					if (m.getSkeleton().getTrackingID() == currentClosest.getSkeleton().getTrackingID() || 
						m.getDevice().getId().equalsIgnoreCase(currentClosest.getDevice().getId())) {
						removeMatches.add(m);
					}
				}
				for (Match m : removeMatches) {
					allComparisons.remove(m);
				}
			}		
		}
		// Notify about close matches.
		for (Match m : closestMatches) {
			callback.accept(m);
		}
	}

	public static double magnitude(Point3d point) {
		return Math.sqrt(Math.pow(point.x, 2) + Math.pow(point.y, 2) + Math.pow(point.z, 2));
	}
	
	public static boolean inSignalRange(AccelerometerData a, AccelerometerData b) {
		if (a == null || b == null || a.getDataPoint() == null || b.getDataPoint() == null) {
			return false;
		}
		double powerA = Math.abs(magnitude(a.getDataPoint()));
		double powerB = Math.abs(magnitude(b.getDataPoint()));
		
		if (powerA < MIN_POWER || powerB < MIN_POWER) {
			return false;
		}
		return Math.abs(powerA - powerB) < MAX_MAGNITUDE_DIFF;
	}
	
	public static boolean inTimeRange(AccelerometerData a, AccelerometerData b) {
		if (a == null || b == null) {
			return false;
		}
		return Math.abs(a.getTrackingTime() - b.getTrackingTime()) < MAX_TIME_DIFF;
	}
}
